#include "taskFlyport.h"

int pos = 1;
int nextPos = 2;
int maxSteps = 60;
int currStep = 0;
int colorPause = 200; // 200 x 10 ms 
int tickTime = 15;

#define 	maxColors	3
const int maxCol = maxColors;
float redVal[maxColors];
float greenVal[maxColors];
float blueVal[maxColors];
float nextR;
float nextG;
float nextB;

void FlyportTask()
{
	// Set open collector output for
	ODCFbits.ODF5 = 1;	// p4
	ODCFbits.ODF2 = 1;	// p6
	ODCBbits.ODB8 = 1;	// p17
	
	// Erase all colors value
	int ii;
	for(ii = 0; ii < maxColors; ii++)
	{
		redVal[ii] = 0;
		greenVal[ii] = 0;
		blueVal[ii] = 0;
	}
	
	WFConnect(WF_DEFAULT);
	while (WFStatus != CONNECTED);
	UARTWrite(1,"Flyport connected... hello world!\r\n");

	// First: setup colors, and turn them off:
	// Red
	PWMInit (1, 1000, redVal[0]);
	PWMOn(p4,1);
	// Green
	PWMInit (2, 1000, greenVal[0]);
	PWMOn(p17,2);
	// Blue
	PWMInit (3, 1000, blueVal[0]);
	PWMOn(p6,3);
	
	
	// After init turn off led5
	vTaskDelay(1000);
	IOPut (p21,off);
	
	DWORD tickMain = TickGetDiv256();
	
	while(1)
	{
		// interpolation between colors...
		// TODO: handle tick overflow!!
		if((TickGetDiv256() - tickMain) > tickTime )
		{			
			if(currStep < maxSteps)
			{
				currStep++;
				// interpolate the pwm values with the currStep index
				
				// RED
				nextR = redVal[pos-1]	+ (((redVal[nextPos-1]   - redVal[pos-1])  *currStep)/maxSteps);

				// GREEN
				nextG = greenVal[pos-1] + (((greenVal[nextPos-1] - greenVal[pos-1])*currStep)/maxSteps);

				// BLUE
				nextB = blueVal[pos-1]  + (((blueVal[nextPos-1]  - blueVal[pos-1]) *currStep)/maxSteps);

				// SET PWMs
				float r, g, b;
				r = ((nextR*100)/255);
				g = ((nextG*100)/255);
				b = ((nextB*100)/255);
				PWMDuty(r, 1);
				PWMDuty(g, 2);
				PWMDuty(b, 3);
			}
			else
			{
				currStep = 0;
				// change position of the colors
				pos++;
				nextPos++;
				
				if(pos > maxColors)
					pos = 1;
				if(nextPos > maxColors)
					nextPos = 1;
					
				// wait a while when the color is reached!
				vTaskDelay(colorPause);
			}
			// Now update the tick value...
			tickMain = TickGetDiv256();
		}
	}
}
